#define _SMAT_RANSAC_CPP_

#include <iostream>
#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"
#include <string.h>


#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>



#include "videoAVI.h"
#include "constants.h"
#include "face_detect_vj.hh"
#include "modelo_RANSAC.h"
#include "smat-ransac.h"
#include "other_functions.h"


int start_SMART_RANSAC_threads(thread_params_t * params_left, thread_params_t * params_right)//, video_params_t * video_params)
{
    pthread_t thl, thr;

    //lanza los hilos que hacen el SMAT y el RANSAC
    if (pthread_create(&thl, NULL, SMAT_RANSAC_thread, params_left))
	return 0;
    if (pthread_create(&thr, NULL, SMAT_RANSAC_thread, params_right))
	return 0;

    params_right->pthread = thr;
    params_left->pthread = thl;

    //lanza el hilo que irá leyendo las imágenes
    //if (pthread_create(&th3, NULL, ReadVideo_thread, video_params))
    //	return 0;
    
    return 1;

}



void * SMAT_RANSAC_thread(void * params)
{
    thread_params_t *p = (thread_params_t*)params;

    while (!p->quit)
    {

	sem_wait(p->sem1);
	if (!SMAT_RANSAC(p))
	    break;
	sem_post(p->sem2);
    }

    return NULL;

}


int SMAT_RANSAC (thread_params_t * p)
{
    CvMat *rotation;
    rotation = cvCreateMat( 3, 3, CV_32FC1 );

    p->src->reset(preprocess_image(p->img_smat), deleteIplImage);	
	   	
    //anulamos los puntos que se pasen del angulo correspondiente
    GetNotHiden(p->vect, p->angY, p->mask_show);

    // Desplazamos los puntos del modelo para que el SMAT use el punto en el centro,
    // no en la esquina.
    CvMat pointsID2;
    cvReshape(p->pointsID, &pointsID2, 2, 0);
    //cvSubS(p->pointsID, cvScalar(p->desplazar_parche,0,0,0), p->pointsID);
    cvSubS(&pointsID2, p->desplazar_parche, &pointsID2);

    /****** SMAT *****/
    cvTranspose(p->pointsID, p->smat_pts->get());

    //set the points for the left/right image
    cvScale( p->smat_pts->get(), p->smat_pts->get(), 0.5);
    (*p->smat)->shape->set_shape(p->smat_pts->get());
    (*p->smat)->track(*p->src, cvPoint(0,0), p->mask_show);
    *p->smat_pts = (*p->smat)->shape->get_shape();
    cvScale(p->smat_pts->get(), p->smat_pts->get(),  2);

    cvTranspose(p->smat_pts->get(), p->pointsID);
	
    // REdesplazamos los puntos del modelo para que el SMAT use el punto en el centro, 
    //no en la esquina.
    //cvAddS(p->pointsID, cvScalar(p->desplazar_parche,0,0,0), p->pointsID);
    cvAddS(&pointsID2, p->desplazar_parche, &pointsID2);

    p->inliers = RANSAC_posit(p->points3D, p->pointsID, p->mask_show, p->maskout, p->cal->KK, 
			      rotation, p->translation, p->vect, &p->error); 
	
    cvReleaseMat(&rotation);

    return 1;
}

int SMAT_RANSAC2 (thread_params_t * p)
{
    CvMat *rotation;
    rotation = cvCreateMat( 3, 3, CV_32FC1 );

	   	
    //anulamos los puntos que se pasen del angulo correspondiente
    GetNotHiden(p->vect_media, p->angY, p->mask_show);
    cvAnd(p->chesscorners, p->mask_show, p->maskout);

    if (1)
    {
	p->src->reset(preprocess_image(p->img_smat), deleteIplImage);	

	// Desplazamos los puntos del modelo para que el SMAT use el punto en el centro,
	// no en la esquina.
	CvMat pointsID2;
	cvReshape(p->pointsID, &pointsID2, 2, 0);
	//cvSubS(p->pointsID, cvScalar(p->desplazar_parche,0,0,0), p->pointsID);
	cvSubS(&pointsID2, p->desplazar_parche, &pointsID2);

	/****** SMAT *****/
	cvTranspose(p->pointsID, p->smat_pts->get());

	//set the points for the left/right image
	cvScale( p->smat_pts->get(), p->smat_pts->get(), 0.5);
	(*p->smat)->shape->set_shape(p->smat_pts->get());
	(*p->smat)->track(*p->src, cvPoint(0,0), p->maskout);
	*p->smat_pts = (*p->smat)->shape->get_shape();
	cvScale(p->smat_pts->get(), p->smat_pts->get(),  2);

	cvTranspose(p->smat_pts->get(), p->pointsID);
	
	// REdesplazamos los puntos del modelo para que el SMAT use el punto en el centro, 
	//no en la esquina.
	//cvAddS(p->pointsID, cvScalar(p->desplazar_parche,0,0,0), p->pointsID);
	cvAddS(&pointsID2, p->desplazar_parche, &pointsID2);
    }
    //busca el punto exacto con el findsubpixel
    FindCornerSubPix(p->img, p->pointsID, p->maskout, 20);

    //ModeloEx(p->points3D, p->pointsID, p->maskout, p->cal->KK, rotation, p->translation, p->vect);
    p->inliers = RANSAC_posit(p->points3D, p->pointsID, p->maskout, p->maskout, p->cal->KK, 
			      rotation, p->translation, p->vect, &p->error); 
	
    cvReleaseMat(&rotation);
    return 1;
}
